#include "Basicpcl.h"
#include "zividcamera.h"

int main(int argc, char **argv)
{

   
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_boundarys(new pcl::PointCloud<pcl::PointXYZ>); 
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("demo.pcd", *cloud) == -1)
    {
        PCL_ERROR("Not  find pcd  \n");
        return (-1);
    }
  //  zivid_camerainit(cloud);
    //Visual_cloudone(cloud);
    // Passfilter(cloud,"x", 0,50,cloud);
    // Passfilter(cloud,"y", 0,100,cloud);
    //Visual_cloudone(cloud_out);


        //提取平面
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
    std::vector<pcl::ModelCoefficients::Ptr> seg_coefficients;
    SegmentModel(cloud, seg_clouds, seg_coefficients, 1, pcl::SACMODEL_PLANE, 1,true);
    pcl::copyPointCloud(*seg_clouds[1],*cloud);

    Statsticalfilter(cloud,cloud);

 //   Visual_cloudone(cloud);
    //提取轮廓
    Boundary_cloud(cloud,cloud_boundarys);
    Visual_cloudone(cloud_boundarys);
    //pcl::io::savePCDFile("line.pcd", *cloud_boundarys);
    
    
    //聚类  删除杂点
    // std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Eucluextra;
    // EuclideCloud(cloud_boundarys,cloud_boundarys,Eucluextra);
    // Visual_clouds(Eucluextra);
    //Statsticalfilter(cloud_boundarys,cloud_boundarys);
    //RadiusFilter(cloud_boundarys,1, 5,cloud_boundarys);

    //提取线
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds_line;
    std::vector<pcl::ModelCoefficients::Ptr> seg_coefficients_line;
    SegmentModel(cloud_boundarys, seg_clouds_line, seg_coefficients_line, 4, pcl::SACMODEL_LINE, 0.8,false);
    

    std::cout<< seg_coefficients_line[0]->values[0]<<","<<seg_coefficients_line[0]->values[1]<<" "<<seg_coefficients_line[0]->values[2]<<" "<<seg_coefficients_line[0]->values[3]<<" "
            <<seg_coefficients_line[0]->values[4]<<" "<<seg_coefficients_line[0]->values[5]<<" "<<seg_coefficients_line[0]->values[6]<<" "<<seg_coefficients_line[0]->values[7]<<seg_coefficients_line[0]->values[8]<<std::endl;
    Visual_clouds(seg_clouds_line);
    std::vector<float> distance ;
    Distance_line(seg_clouds_line, distance);
    //seg_coefficients_line 保存数据  1 2 是长线   3 4短线     1线 垂直于 3 4线所在平面   求交点  2个交点 求距离
    //求直线到面交点 
     std::vector<pcl::PointXYZ> points_line;
    Line_Plane_point(seg_coefficients_line, points_line);
    // boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
    // viewer = SimpleVIS(cloud_boundarys,cloud_boundarys,cloud);
    // while (!viewer->wasStopped()){
    //     viewer->spinOnce();
    // }

    return 0;
}